#include "zuc_nml_rob_cmd.hh"
#include "libnml/cms/cms.hh"
#include "zuc/nml_intf/zuc_nml_rob.hh"

void ZUC_ROB_CMDITF::update(CMS* cms)
{
    cms->update(robot_id);
    update_param(cms);
}
void ZUC_JOG_CMD_MSG::update_param(CMS* cms) { cms->update(joint_or_axis); }
void ZUC_JOG_CONT::update_param(CMS* cms)
{
    cms->update(vel);
    cms->update(jjogmode);
}

void ZUC_JOG_INCR::update_param(CMS* cms)
{
    cms->update(incr);
    cms->update(vel);
    cms->update(jjogmode);
}

void ZUC_JOG_ABS::update_param(CMS* cms)
{
    cms->update(pos);
    cms->update(vel);
    cms->update(jjogmode);
}

void ZUC_JOG_MULTI_JOINT::update_param(CMS* cms)
{
    RobJointVal_update(cms, &pos);
    cms->update(vel);
}

void ZUC_JOG_MOVJ::update_param(CMS* cms)
{
    cms->update(mvOpt);
    RobJointVal_update(cms, &pos_cmd);
    cms->update(vel);
    cms->update(acc);
    cms->update(tol);
    cms->update(end_cond[0]);
    cms->update(end_cond[1]);
    cms->update(end_cond[2]);
    cms->update(executing_line_id);
    cms->update(is_cpos);
}

void ZUC_JOG_MOVL::update_param(CMS* cms)
{
    cms->update(mvOpt);
    cms->update(pos_cmd, ZUCMOT_MAX_JOINTS);
    cms->update(vel);
    cms->update(acc);
    cms->update(tol);
    cms->update(end_cond, 3);
    cms->update(executing_line_id);
}

void ZUC_JOG_MOVC::update_param(CMS* cms)
{
    cms->update(mvOpt);
    cms->update(pos_mid, ZUCMOT_MAX_JOINTS);
    cms->update(pos_end, ZUCMOT_MAX_JOINTS);
    cms->update(vel);
    cms->update(acc);
    cms->update(tol);
    cms->update(circle_cnt);
    cms->update(end_cond, 3);
    cms->update(executing_line_id);
}

void ZUC_JOG_STOP::update_param(CMS* cms) { cms->update(jjogmode); }
void Robot_7_DOF_INV_INFORM::update_param(CMS* cms)
{
    cms->update(inv_flag);
    cms->update(arm_angle);
}
void Robot_SET_7_DOF_KINE_PARAM_ALPHA_K::update_param(CMS* cms)
{
    cms->update(alpha);
    cms->update(k);
}
void ZUC_SET_CUSTOM_POSE::update_param(CMS* cms)
{
    cms->update(type);
    RobJointVal_update(cms, &jpos);
    cms->update(poseTol);
}

void ZUC_SET_SERVOJ_PARAMETER::update_param(CMS* cms)
{
    cms->update(type);
    cms->update(servo_filter_type);
    cms->update(mmf_buf_size);
    cms->update(mmf_ka);
    cms->update(mmf_kv);
    cms->update(mmf_kp);
    cms->update(nlf_vp);
    cms->update(nlf_ap);
    cms->update(nlf_jp);
    cms->update(nlf_vr);
    cms->update(nlf_ar);
    cms->update(nlf_jr);
    cms->update(lpf_cutoff_freq);
}

void ZUC_SERVO_DYNAMICS_PARA::update_param(CMS* cms) { cms->update(servo_dynamics_para, 14 * ZUCMOT_MAX_JOINTS); }

void ZUC_SERVO_FRICTION::update_param(CMS* cms)
{
    cms->update(friction_para, 14 * ZUCMOT_MAX_JOINTS);
    cms->update(jnt_id);
}

void ZUC_COLLISION_SENSITIVITY::update_param(CMS* cms) { cms->update(joint_sensitivity, ZUCMOT_MAX_JOINTS); }

void ZUC_BACKDRIVE_ON::update_param(CMS* cms) { cms->update(enable); }

void ZUC_TRAJ_SET_MAX_VELOCITY::update_param(CMS* cms) { cms->update(velocity); }

void ZUC_TRAJ_SET_SCALE::update_param(CMS* cms) { cms->update(scale); }

void ZUC_TRAJ_SET_RAPID_SCALE::update_param(CMS* cms) { cms->update(scale); }
void ZUC_TRAJ_LINEAR_MOVE::update_param(CMS* cms)
{
    ZucPose_update(cms, &end);
    cms->update(type);
    cms->update(vel);
    cms->update(ini_maxvel);
    cms->update(acc);
    cms->update(feed_mode);
    cms->update(indexrotary);
    cms->update(jerk);
    cms->update(using_abc);
    cms->update(di_type);
    cms->update(di_index);
    cms->update(di_state);
    cms->update(planner_type);
    cms->update(tool_id);
    cms->update(usrframe_id);
    cms->update(payload_id);
    cms->update(blend_mode);
    cms->update(blend_tol);
}
void ZUC_TRAJ_CIRCULAR_MOVE::update_param(CMS* cms)
{
    ZucPose_update(cms, &end);
    cms->update(center);
    cms->update(normal);
    cms->update(turn);
    cms->update(type);
    cms->update(vel);
    cms->update(ini_maxvel);
    cms->update(acc);
    cms->update(feed_mode);
    cms->update(jerk);

    cms->update(tool_id);
    cms->update(usrframe_id);
    cms->update(payload_id);
    cms->update(blend_mode);
    cms->update(blend_tol);
}

void ZUC_TRAJ_MOVC::update_param(CMS* cms)
{
    ZucPose_update(cms, &midPoint);
    ZucPose_update(cms, &endPoint);
    cms->update(vel);
    cms->update(ini_maxvel);
    cms->update(acc);
    cms->update(jerk);
    cms->update(using_abc);
    cms->update(circle_count);
    cms->update(di_type);
    cms->update(di_index);
    cms->update(di_state);
    cms->update(planner_type);

    cms->update(tool_id);
    cms->update(usrframe_id);
    cms->update(payload_id);
    cms->update(blend_mode);
    cms->update(blend_tol);
}

void ZUC_TRAJ_JOINT_MOVE::update_param(CMS* cms)
{
    ZucPose_update(cms, &end);
    cms->update(type);
    cms->update(vel);
    cms->update(ini_maxvel);
    cms->update(acc);
    cms->update(jerk);
    cms->update(feed_mode);
    cms->update(indexrotary);
    cms->update(jointNum);
    cms->update(di_type);
    cms->update(di_index);
    cms->update(di_state);
    cms->update(planner_type);

    cms->update(blend_mode);
    cms->update(blend_tol);
}

void ZUC_TRAJ_MOVS::update_param(CMS* cms)
{
    cms->update(type);
    cms->update(movs_a0, 6);
    cms->update(movs_a1, 6);
    cms->update(movs_a2, 6);
    cms->update(movs_a3, 6);
    cms->update(movs_time);
}

void ZUC_SET_USER_ID::update_param(CMS* cms) { cms->update(id); }
void ZUC_SET_TOOL_ID::update_param(CMS* cms) { cms->update(id); }

void ZUC_CONVEYOR_ENABLE::update_param(CMS* cms)
{
    cms->update(convyr_type);
    cms->update(x);
    cms->update(y);
    cms->update(z);
    cms->update(p1x);
    cms->update(p1y);
    cms->update(p1z);
    cms->update(p2x);
    cms->update(p2y);
    cms->update(p2z);
    cms->update(p3x);
    cms->update(p3y);
    cms->update(p3z);
    cms->update(pulseEquivalent);
    cms->update(rotate_tool);
    cms->update(maxDistance);
}

void ZUC_TRAJ_SET_ROBOT_TOOL_OFFSET::update_param(CMS* cms) { ZucPose_update(cms, &robotToolOffset); }

void ZUC_TRAJ_SET_ROBOT_USER_FRAME::update_param(CMS* cms) { ZucPose_update(cms, &robotUserFrame); }

void ZUC_TRAJ_CALIB_DHPARAM::update_param(CMS* cms) { DHParam_update(cms, &dhParamError); }

void ZUC_TRAJ_SET_HOME::update_param(CMS* cms) { ZucPose_update(cms, &home); }

void ZUC_TRAJ_SET_TELEOP_ENABLE::update_param(CMS* cms) { cms->update(enable); }

void ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE::update_param(CMS* cms) { cms->update(enable); }

void ZUC_TRAJ_SET_SERVOJOP_ENABLE::update_param(CMS* cms) { cms->update(enable); }

void ZUC_TRAJ_SET_ADMITOP_ENABLE::update_param(CMS* cms) { cms->update(enable); }

void ZUC_TRAJ_SET_COMPLIANCE_ENABLE::update_param(CMS* cms)
{
    cms->update(compliantType);
    cms->update(compliantEnable);
}
void ZUC_TRAJ_SET_ADMITTANCE_ENABLE::update_param(CMS* cms) { cms->update(enable); }

void ZUC_TRAJ_DISABLE_FORCE_CONTROL::update_param(CMS* cms) { cms->update(disableForceCtrl); }

void ZUC_TRAJ_SET_ADMITTANCE_CONFIG::update_param(CMS* cms)
{
    cms->update(axis);
    cms->update(now);
    FtConfig_update(cms, &ftConfig);
}

void ZUC_TRAJ_SET_ADMITTANCE_OPTION::update_param(CMS* cms) { cms->update(admit_axis_opt, CART_DIM); }
void ZUC_TRAJ_SET_END_FORCE_CONDITION::update_param(CMS* cms)
{
    cms->update(axis);
    cms->update(endForceCond.enableEndCond);
    cms->update(endForceCond.lowFlag);
    cms->update(endForceCond.upFlag);
    cms->update(endForceCond.lowLimitForce);
    cms->update(endForceCond.upLimitForce);
}

void ZUC_TRAJ_RIGID_TAP::update_param(CMS* cms)
{
    ZucPose_update(cms, &pos);
    cms->update(vel);
    cms->update(ini_maxvel);
    cms->update(acc);
}

void ZUC_TRAJ_SET_ADMITTANCE_FRAME::update_param(CMS* cms) { cms->update(ftFrame); }

void ZUC_SET_PLANNER_JERK::update_param(CMS* cms)
{
    cms->update(joint_jerk);
    cms->update(carte_jerk);
}

void ZUC_SET_DRAG_ENABLE::update_param(CMS* cms) { cms->update(enable); }

void ZUC_SET_ROBOT_PAYLOAD::update_param(CMS* cms) { cms->update(id); }

void ZUC_SERVO_HOME::update_param(CMS* cms) { cms->update(joint); }

void ZUC_ADMITTANCE_DRAG_DEAD_ZONE::update_param(CMS* cms)
{
    cms->update(Fx);
    cms->update(Fy);
    cms->update(Fz);
    cms->update(Mx);
    cms->update(My);
    cms->update(Mz);
}

void ZUC_TORQUE_SENSOR_SOFT_LIMIT::update_param(CMS* cms)
{
    cms->update(Fx);
    cms->update(Fy);
    cms->update(Fz);
    cms->update(Mx);
    cms->update(My);
    cms->update(Mz);
}

void ZUC_TORQUE_SENSOR_FILTER::update_param(CMS* cms) { cms->update(torqueSensorFilter); }

void ZUC_TORQSENSOR_SETMODE::update_param(CMS* cms) { cms->update(mode); }

void ZUC_TORQSENSOR_SETBRAND::update_param(CMS* cms) { cms->update(torqsensorBrand); }

void ZUC_MOTION_SET_HOME_VEL::update_param(CMS* cms)
{
    cms->update(setFlag);
    cms->update(homeVel);
}

void ServoParam_update(CMS* cms, ServoParam* x)
{
    // unsigned char* pMem = static_cast<unsigned char*>(x);
    cms->update((unsigned char*)(x), sizeof(ServoParam));
}

void ZUC_SET_SERVO_PARAM::update_param(CMS* cms) { ServoParam_update(cms, &servoParam); }

void ZUC_SET_BASE_OFFSET::update_param(CMS* cms) { cms->update(base_offset, 6); }

void ZUC_SET_SIM_ROBOT::update_param(CMS* cms) { cms->update(robotModel); }